/*
The surface-physics project: http://code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0

Bump_Plate.cpp
Bump_Plate management implementation.


*/


/* Class declaration */
#include "Bump_Plate.h"


DerivedRegister<Bump_Plate> Bump_Plate::reg("Bump_Plate");
int Bump_Plate::count = 0;

/**
 * Constructor for this derived class
 */
Bump_Plate::Bump_Plate()
{
	

}


/**
 * Destructor, does nothing currently
 */
Bump_Plate::~Bump_Plate()
{}


/**
 * Initialize a base component.
 *
 *
 */
int
Bump_Plate::init(Bump_BulletBase *bb, char *strObjName,  OBJHANDLE hObj)
{
	int rv;

	bumpBase = bb;
	strcpy(strObjectName, strObjName);

	sprintf(boutput, "Bump_Plate::init: Initializing %s", strObjectName);
	oapiWriteLog(boutput);

	//Initialize position
	//TODO: this should be read from a file
	rposCurrent = _V(104.f, 30.f, -174.5f);

	//Create Orbiter object: Load any meshes by reading the mesh files from the physics file
	rv = createOrbiterPrimitives();
	if(rv){
		sprintf(boutput, "Bump_Plate::init: ERROR : Creating Orbiter primitives failed %s",
				strObjectName);
		oapiWriteLog(boutput);
		return 1;
	}


	//Create terrain patch from initial equatorial position of vessel
	if(bumpBase->terrainGenerationEnabled){
		ground = new Bump_TerrainPatch(4, 4, 0.000002f, 0.000002f, 0.000003f, 0.000003f, 10.f);

		if(!ground){
			sprintf(boutput, "Bump_Plate::init: ERROR : Could not add ground for %s", strObjectName);
			oapiWriteLog(boutput);
			return 2;

		}

		//bumpBase->bb->Local2Global(rposCurrent, gposCurrent);
		double lng, lat, rad;
		//oapiLocalToEqu (bumpBase->hObjBulletBase, rposCurrent, &lng, &lat, &rad);

		bumpBase->bb->Local2Global(rposCurrent, gposCurrent);
		oapiGlobalToEqu(bumpBase->hObjRefBody, gposCurrent, &lng, &lat, &rad);

		rv = ground->createPatch(bumpBase, lng, lat);
		if(rv){
			sprintf(boutput, "Bump_Plate::init: ERROR : Could not add ground for %s : %d", strObjectName, rv);
			oapiWriteLog(boutput);
			return 3;
		}
	}



	//Create physics counterpart, if this fails, should remove Orbiter meshes that were loaded above
	rv = createBulletRigidBody();
	if(rv){

		destroyOrbiterPrimitives();

		sprintf(boutput, "Bump_Plate::init: ERROR : Creating rigid body failed %s",
				strObjectName);
		oapiWriteLog(boutput);
		return 1;
	}


	//Only after Orbiter AND Bullet successful, set physics to true
	physicsInited = true;

	count++;
	return 0;
}


/**
 * Updates a base mesh
 * Primarily move the mesh into position according to latest transform by setting
 * translation, yaw ,pitch & roll using animation handles created in init().
 *
 */
int
Bump_Plate::poststep(double SimT, double SimDT, double MJD)
{

	double state = 0.0f;



	//sprintf(boutput, "Bump_Plate::update: The next sim step will advance the sim by %f secs", SimDT);
	//oapiWriteLog(boutput);

	if(physicsInited){

		//Get the physics transform

		rbPlate->getMotionState()->getWorldTransform(transComponent);
		//trans.getOpenGLMatrix(m);
		originComponent = transComponent.getOrigin();
		x = originComponent.x();
		y = originComponent.y();
		z = originComponent.z();
		transComponent.getBasis().getEulerYPR(rz, ry, rx);

		//sprintf(output,"Sphere height: %f, rx=%f, ry=%f, rz=%f\n", trans.getOrigin().getY(), rx*180/PI, ry*180/PI, rz*180/PI);
		//oapiWriteLog(output);



		//Update the Orbiter mesh
		state = fabs(x/Bump_BulletBase::BULLET_WORLD_SIZE);
		state *= 0.5; //reduce range to within  0 to 0.5 or 0.5 to 1
		state = (x > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
		bumpBase->bb->SetAnimation(animTX, state);

		state = fabs(y/Bump_BulletBase::BULLET_WORLD_SIZE);
		state *= 0.5; //reduce range to within  0 to 0.5 or 0.5 to 1
		state = (y > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
		bumpBase->bb->SetAnimation(animTY, state);

		state = fabs(z/Bump_BulletBase::BULLET_WORLD_SIZE);
		state *= 0.5; //reduce range to within  0 to 0.5 or 0.5 to 1
		state = (z > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
		bumpBase->bb->SetAnimation(animTZ, state);

		state = fabs((rz+PI)/(2*PI) );
				bumpBase->bb->SetAnimation(animRZ, state);

		state = fabs((-ry+PI)/(2*PI)); //Very important to negate RY
								bumpBase->bb->SetAnimation(animRY, state);


		state = fabs((rx+PI)/(2*PI));
		bumpBase->bb->SetAnimation(animRX, state);


		//Update terrain
		if(bumpBase->terrainGenerationEnabled){

			//oapiEquToGlobal(base->hObjRefBody, lng, lat, base->rad, &gposSurfPoint);
			//oapiLocalToEqu(bumpBase->hObjRefBody, _V(m[12], m[13], m[14]), &lng, &lat, &rad);

			bumpBase->bb->Local2Global(_V(x, y,z), gposCurrent);
			oapiGlobalToEqu(bumpBase->hObjRefBody, gposCurrent, &lng, &lat, &rad);

			/*sprintf(boutput, "Bump_Plate::poststep: Converted (%f,%f,%f) to lolara(%f,%f,%f)",
					m[12], m[13], m[14], lng, lat, rad);
			oapiWriteLog(boutput);*/

			int t = ground->updatePatch(bumpBase, lng, lat, rad);
		}





	}//physics inited



	return 0;
}


/**
 *	Process keyboard input any other messages
 */
int
Bump_Plate::processCommand()
{
/*	if(bbv->validKeyInput){

		//Command
		switch (bbv->keyInput.key)
		{
			case OAPI_KEY_UP:

				break;

			case OAPI_KEY_SPACE:
				break;



			case NO_KEY:

				break;

			default:
				break;
		}
	}
*/
	return 0;
}


/**
 * Deallocates any memory, Bump_Plate::exit() will be called
 * by the containing Bump_BulletBase.
 *
 */
int
Bump_Plate::exit()
{
	int rv;


	//Destroy physics counterpart
	rv = destroyBulletRigidBody();
	if(rv){
		sprintf(boutput, "Bump_Plate::exit: ERROR : Destroying rigid body failed for %s", strObjectName);
		oapiWriteLog(boutput);
	}


	//Remove Orbiter handles, update base
/*	rv = detachOrbiterVESSEL();
	if(rv){
		sprintf(boutput, "Bump_Plate::exit: ERROR : Detaching Orbiter VESSEL failed %s", strObjectName);
		oapiWriteLog(boutput);
	}*/

	count--;

	//Base class members cleanup
	Bump_BaseObject::exit();

	return rv;

}


/**
 * Only resets the physics body. There is no need to re-attach
 *
 * Currently complete vehicle memory de-allocation is disabled due to m_actions[] in the
 * btDiscreteDynamicsWorld not being properly resized m_actions.size() fails in an assertion.
 *
 * This has to be reproduced in the Vehicle demo and asked in the Bullet forums.
 * Also test with re-spawn(reqd for Orulex) & deallocate memory options in Orbiter debugging options.
 *
 *
 * Meanwhile reset() causes the vehicle to turn back upright if toppled.
 *
 * TODO: Causes crash, fix destroyBulletRigidBody(),
 * Test in VehicleDemo
 */
int
Bump_Plate::reset()
{




	return 0;
}



/**
 * Creates the animation handles with the rotations around the axes as child anims
 * of the translations.
 *
 */
int
Bump_Plate::createOrbiterPrimitives()
{
	//Load the mesh
	VECTOR3 ofs = _V(0.f, 0.f, 0.f);
	mshIndex = bumpBase->bb->AddMesh("Bump/ComponentLibrary/Plate", &ofs);


	//Animation setup
	static UINT group[] = {GROUP_INDEX}; // participating groups

	static MGROUP_TRANSLATE mgtTX( mshIndex, group, 0, _V(2*Bump_BulletBase::BULLET_WORLD_SIZE, 0, 0));
	animTX = bumpBase->bb->CreateAnimation (0.5);
	parent = bumpBase->bb->AddAnimationComponent (animTX, 0, 1, &mgtTX);

	mgtTY = new MGROUP_TRANSLATE( mshIndex, group, 0, _V(0, 2*Bump_BulletBase::BULLET_WORLD_SIZE, 0));
	animTY = bumpBase->bb->CreateAnimation (0.5);
	parent = bumpBase->bb->AddAnimationComponent (animTY, 0, 1, mgtTY, parent);

	mgtTZ = new MGROUP_TRANSLATE( mshIndex, group, 0, _V(0, 0, 2*Bump_BulletBase::BULLET_WORLD_SIZE));
	animTZ = bumpBase->bb->CreateAnimation (0.5);
	parent = bumpBase->bb->AddAnimationComponent (animTZ, 0, 1, mgtTZ, parent);

	mgrRZ = new MGROUP_ROTATE( mshIndex, group, 0, _V(0, 0, 0), _V(0, 0, 1), (float)(2 * double(PI) ) );
	animRZ = bumpBase->bb->CreateAnimation (0.0);
	parent = bumpBase->bb->AddAnimationComponent (animRZ, 0, 1, mgrRZ, parent);

	mgrRY = new MGROUP_ROTATE( mshIndex, group, 0, _V(0, 0, 0), _V(0, 1, 0), (float)(2 * PI) );
	animRY = bumpBase->bb->CreateAnimation (0.0);
	parent = bumpBase->bb->AddAnimationComponent (animRY, 0, 1, mgrRY, parent);

	mgrRX = new MGROUP_ROTATE( mshIndex, group, 1, _V(0, 0, 0), _V(1, 0, 0), (float)(2 * PI) );
	animRX = bumpBase->bb->CreateAnimation (0.0);
	parent = bumpBase->bb->AddAnimationComponent (animRX, 0, 1, mgrRX, parent);


	return 0;
}


/**
 * Delete mesh and animation handles.
 *
 */
int
Bump_Plate::destroyOrbiterPrimitives()
{
	bool rv;

	rv = bumpBase->bb->DelMesh(mshIndex);
	if(rv){
		sprintf(boutput, "Bump_Plate::destroyOrbiterPrimitives: ERROR : Destroying Orbiter mesh(%d) failed for %s",
				mshIndex, strObjectName);
		oapiWriteLog(boutput);
		return 1;
	}

	//Not needed as animations of the mesh deleted above, are also deleted when mesh is deleted.
	/*rv = bumpBase->bb->DelAnimation(animTX);
	if(rv){
		sprintf(boutput, "Bump_Plate::destroyOrbiterPrimitives: ERROR : Destroying Orbiter animation failed for %s", strObjectName);
		oapiWriteLog(boutput);
		return 1;
	}*/

	return 0;
}


/**
 * Create the physics counterpart in the Bullet world.
 *
 * Preferably get all info required from Orbiter and store in members beforehand.
 *
 * Optimize: reuse box shape by scaling a unit block for the pad sections and blocks
 *
 * TODO: Parse the base cfg file to get the shapes instead of hard coding ? Or merge
 * 		 base cfg with physics file for base ?
 *
 */
int
Bump_Plate::createBulletRigidBody()
{

	btTransform trans;
	btQuaternion q;
	btVector3 plateInertia(0,0,0);


	trans.setIdentity();
	trans.setOrigin(btVector3(btScalar(rposCurrent.x),
							  btScalar(rposCurrent.y),
							  btScalar(rposCurrent.z))); //at pad 2 of Brighton Beach
	q.setEuler(0.f, 0.f, 0.f);
	trans.setRotation(q);
	mass = 1.0f;

	btCollisionShape* boxshpPlate = new btBoxShape(btVector3(2.5f, 0.05f, 2.5f));
	bumpBase->m_collisionShapes->push_back(boxshpPlate);

	boxshpPlate->calculateLocalInertia(mass, plateInertia);
	btDefaultMotionState* plateMotionState = new btDefaultMotionState(trans);
	btRigidBody::btRigidBodyConstructionInfo rbciBall(mass, plateMotionState, boxshpPlate, plateInertia);
	rbPlate = new btRigidBody(rbciBall);
	bumpBase->m_dynamicsWorld->addRigidBody(rbPlate);


	rbPlate->setGravity(btVector3((btScalar)bumpBase->G.x,
										   (btScalar)bumpBase->G.y,
										   (btScalar)bumpBase->G.z));


	return 0;
}


/**
 * De-allocates memory being consumed by the raycast vehicle so it can be
 * created again by createBulletRigidBody()
 *
 * This is a very important function. If memory is not freed up, Orbiter sim will not
 * exit properly and the Launchpad will not appear leading to user frustration!
 *
 * Here we de-allocate only the sub class specific stuff which have not been freed by
 * bump.clientResetScene(), which is called before Bump_BulletBase::exit();
 *
 * Thus rigid bodies(derived from CollisionObjects), motion states are already removed
 * by the time we come here, DO NOT remove them again !
 *
 * The stuff which should be removed are any sub class specific classes used to model
 * this object like m_vehicleRayCaster & m_vehicle. Also any collision shapes not pushed
 * into m_collisionShapes[] during init(), needs to be freed here, such as the wheelShape
 */
int
Bump_Plate::destroyBulletRigidBody()
{
	physicsInited = false;



	return 0;
}
